


import lejos.nxt.UltrasonicSensor;
import lejos.robotics.proposal.DifferentialPilot;
import lejos.robotics.subsumption.Behavior;
import java.lang.Math.*;

public class CorrectToParallel implements Behavior{
	private boolean suppressed = false;
	private float distanceToWall;
	private float tolerance;
	private DifferentialPilot pilot;
	private UltrasonicSensor ultraSonicSensor;
	private final int correctionAngle = 5;

	
	public CorrectToParallel(float distanceToWall, float tolerance, DifferentialPilot pilot , UltrasonicSensor ultrasonicSensor){
		this.distanceToWall= distanceToWall;
		this.tolerance = tolerance;
		this.pilot = pilot;
		this.ultraSonicSensor = ultraSonicSensor;		
	}
	
	@Override
	public boolean takeControl() {
		return !isParrallelToWall(distanceToWall, tolerance);
	}
	
	@Override
	public void action() {		 
		float fout = ultraSonicSensor.getDistance() - distanceToWall;
		if(fout > 0){ //Robot rijdt te ver van de muuur
			/**  /  /  /   /   /   /   /   /   / 
			 * ----------------muur---------------------
			 * ____________ _ _ _ _ te volgen weg
			 *            \__________________ te corrigeren huidige weg
			 */	
			pilot.rotate(correctionAngle);
			float correctionDistance = (float) (fout/Math.cos(correctionAngle));
			pilot.travel(correctionDistance);
			pilot.rotate(-correctionAngle);
		}else{ //Robot rijdt te dicht
			//TODO invullen
		}
	}

	@Override
	public void suppress() {
		pilot.stop();
		this.suppressed = true;
		
	}

	private boolean isParrallelToWall(double distanceToWall, double tolerance){
		return (ultraSonicSensor.getDistance() < distanceToWall + tolerance) && (ultraSonicSensor.getDistance() > distanceToWall - tolerance);
	}

}
